﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;

using KSASupport.Calibration;
using System.Threading.Tasks;
using System.Windows;


namespace KSASupport.Calibration
{
    /// <summary>
    /// Responsible for Storing off calibration data, filtering it, and returning
    /// the 'active area' back to the Depth manager for analysis
    /// </summary>
    public class DepthCalibrationManager
    {

        private CalibrationPixel[] calibrationGrid;
        // stores each calibration pixels settings and values.
        public CalibrationPixel[] CalibrationGrid
        {
            get { return calibrationGrid; }
            set { calibrationGrid = value; }
        }
        /// <summary>
        /// Byte image representation of the calibratio grid. Useful for analyzing a calibration after the fact
        /// </summary>
        public byte[] CalibrationImage;
        /// <summary>
        /// Array of the depth data used for this calibration
        /// </summary>
        public short[] DepthData;

        /// <summary>
        /// Tracks if we're in the process of calibrating
        /// </summary>
        public bool IsCalibrating = false;

        /// <summary>
        /// Tracks if the unit is calibrated or not
        /// </summary>
        public bool IsCalibrated = false;

        /// <summary>
        /// Store this off to look up touch configuration settings
        /// </summary>
        private DepthManagerSettings depthManagerSettings;

        // The following track consistency of measurements when using 4 point calibrations
        // It's important to get the same values several times in a row

        int TopLeftHeight;
        int TopRightHeight;
        int BottomLeftHeight;
        int BottomRightHeight;
        private int CalibrationDepthRectConsistency = 0; // number of times we've seen the same rect above

        /// <summary>
        /// Instantiates a new instance of the Depth Calibration Manager
        /// </summary>
        /// <param name="settings">An instance of the settings to use for storing/loading saved values</param>
        public DepthCalibrationManager(DepthManagerSettings settings)
        {
            depthManagerSettings = settings;

            try
            {
                if (settings.calibrationDistanceValues != null)
                {
                    ReadCalibration();
                }
            }
            catch (Exception)
            { 
                // silently swallow if we have any problems at this point.
                IsCalibrated = false;
            }
        }

        /// <summary>
        /// Reads from the depthmanagersettings values, and fills out our calibrated values
        /// </summary>
        internal void ReadCalibration()
        {
            IsCalibrated = depthManagerSettings.ReadSettings();
            if (!IsCalibrated)
            {
                return;
            }
            calibrationGrid = new CalibrationPixel[depthManagerSettings.calibrationDistanceValues.Length];
            depthManagerSettings.calibrationDistanceValues.CopyTo(calibrationGrid, 0);
        }

        /// <summary>
        /// Start the calibratin process. Note: The kinect should have a solid image coming back to it at this point. Please either cover the device with paper, or
        /// use a projector or device with a much less reflective background
        /// </summary>
        public void StartCalibration()
        {            
            calibrationGrid = null;
            IsCalibrating = true;
        }

        /// <summary>
        /// Stops the calibration process, and saves the calibration off to a file.
        /// </summary>
        public void StopCalibration()
        {
            IsCalibrating = false;
            depthManagerSettings.calibrationDistanceValues = new CalibrationPixel[depthManagerSettings.DepthResolutionWidth * depthManagerSettings.DepthResolutionHeight];
            calibrationGrid.CopyTo(depthManagerSettings.calibrationDistanceValues, 0);
            depthManagerSettings.SaveSettings();
            IsCalibrated = true;
        }
        #region Experimental 'plane' calibration method

        /// <summary>
    /// This calibration method is for 'flat' displays (most displays). This will use 4 corner points, and calculate
    /// the depth across the display. This allows the user to quickly calibrate with just 4 points vs. having to lay down
    /// something over the surface for 'each' point. For reflective screens this should be faster and more accurate.
    /// Note: Currently not using this method, as I've not gotten better results from it at this time
    /// </summary>
    /// <param name="imageFrame"></param>
    /// <param name="topLeft"></param>
    /// <param name="bottomLeft"></param>
    /// <param name="bottomRight"></param>
    /// <param name="topRight"></param>
        public void ProcessImageFrameForCalibration(DepthImageFrame imageFrame, 
            Point topLeft, 
            Point bottomLeft,
            Point bottomRight,
            Point topRight)
        {
            if (calibrationGrid == null)
            {
                calibrationGrid = new CalibrationPixel[imageFrame.PixelDataLength];
                CalibrationImage = new byte[imageFrame.PixelDataLength * 4];
                DepthData = new short[imageFrame.PixelDataLength];
            }

            imageFrame.CopyPixelDataTo(DepthData);
            // Note: Use 4 points from bezel to establish a 'zero' height.  The user can adjust this up or 
            // down later. Since the glass of a screen would match the bezel, as long as we establish a 'plane' we
            // should be good to go. We may have to filter the bezel out afterwards.
            if (CalibrationDepthRectConsistency == 0)
            {
                TopLeftHeight = DepthData[(int)((topLeft.Y * imageFrame.Width) + topLeft.X)]                    >> DepthImageFrame.PlayerIndexBitmaskWidth;
                TopRightHeight = DepthData[(int)((topRight.Y * imageFrame.Width) + topRight.X)]                 >> DepthImageFrame.PlayerIndexBitmaskWidth;
                BottomLeftHeight = DepthData[(int)((bottomLeft.Y * imageFrame.Width) + bottomLeft.X)]           >> DepthImageFrame.PlayerIndexBitmaskWidth;
                BottomRightHeight = DepthData[(int)((bottomRight.Y * imageFrame.Width) + bottomRight.X)]        >> DepthImageFrame.PlayerIndexBitmaskWidth;
                CalibrationDepthRectConsistency++;
            }
            else
            {
                if ((TopLeftHeight  == DepthData[(int)((topLeft.Y * imageFrame.Width) + topLeft.X)]             >> DepthImageFrame.PlayerIndexBitmaskWidth) &&
                 (TopRightHeight    == DepthData[(int)((topRight.Y * imageFrame.Width) + topRight.X)]           >> DepthImageFrame.PlayerIndexBitmaskWidth) &&
                 (BottomLeftHeight  == DepthData[(int)((bottomLeft.Y * imageFrame.Width) + bottomLeft.X)]       >> DepthImageFrame.PlayerIndexBitmaskWidth) &&
                 (BottomRightHeight == DepthData[(int)((bottomRight.Y * imageFrame.Width) + bottomRight.X)]     >> DepthImageFrame.PlayerIndexBitmaskWidth))
                {
                    CalibrationDepthRectConsistency++;
                }
                else
                {
                    CalibrationDepthRectConsistency = 0;
                }
            }

            // Check to see if we have consistent data yet..
            if (CalibrationDepthRectConsistency > 6)
            {
                // First up, calculate out our Y values from Topleft to bottom left, and top right to bottom right
                // Then, go across from side to side, plotting the expected depth of each value, and storing the
                // depth in the XY container
                // Anything left over that is null can be left, and it'll get taken care of by the serializing 
                // algo.
                // Calculate formula of vertical lines for each side...
                int[] LeftSideValues = new int[(int)Math.Abs(topLeft.Y - bottomLeft.Y)];
                double x1 = topLeft.Y;
                double y1 = TopLeftHeight;
                double x2 = bottomLeft.Y;
                double y2 = BottomLeftHeight;

                double mSlope = (y1 - y2) / (x1 - x2);
                double b = y1 - (mSlope * x1);
                // y = (mSlope * X) + b
                for (int i = 0; i < LeftSideValues.Count(); i++)
                { 
                    int tempX = (int)topLeft.Y + i;
                    LeftSideValues[i] = (int)((tempX * mSlope) + b);
                }

                // Calculate same for the right side
                int[] RightSideValues = new int[(int)Math.Abs(topRight.Y - bottomRight.Y)]; 
                Point p1r = new Point(topRight.Y,TopRightHeight);
                Point p2r = new Point(bottomRight.Y,BottomRightHeight);
                double mSloper = GetSlope(p1r,p2r);
                double br = GetYIntercept(p1r,mSloper);

                // y = (mSlope * X) + b
                for (int i = 0; i < RightSideValues.Count(); i++)
                { 
                    int tempXr = (int)topRight.Y + i;
                    RightSideValues[i] = (int)((tempXr * mSloper) + br);
                }

                // Now, for each value in leftsidevalues AND rightside values:
                // 1) Find index to start from leftside X/Y value
                // 2) Find Slope and Y intercept of the line from Leftside to rightside
                // 3) Plot line values of depth into CalibrationGrid based on slope of line
                // 4) Repeat 2-3 through all values along left to right side

                for (int i = 0; i < (int)depthManagerSettings.ScreenBoundaryRect.Height; i++)
                {
                    // Find our leftmost point, and the depth of it.
                    Point currentLeft = new Point(topLeft.X,
                        topLeft.Y + i);
                    int currentLeftDepth = LeftSideValues[i];

                    // Find the rightmost point, and it's depth
                    Point currentRight = new Point(topRight.X,
                        currentLeft.Y);
                    int currentRightDepth = RightSideValues[i];

                    // Switch dimensions...find the slope along the X axis of the depth
                    Point leftPoint = new Point(currentLeft.X, currentLeftDepth);
                    Point rightPoint = new Point(currentRight.X, currentRightDepth);

                    double slope = GetSlope(leftPoint, rightPoint);
                    double yInt = GetYIntercept(leftPoint, mSlope);
                    int startIndex = (int)((currentLeft.Y * depthManagerSettings.DepthResolutionWidth) + currentLeft.X);
                    
                    // Now that we have the data to formulate the line, assign indexes across
                    // the X axis for this Y position with calculated depth values
                    for (int j = 0; j < depthManagerSettings.ScreenBoundaryRect.Width;j++ )
                    {
                        int depth = (int)((currentLeft.X + j) * slope + yInt);
                        calibrationGrid[startIndex + j] = new CalibrationPixel(depth,depthManagerSettings);
                    }
                }
                StopCalibration();
            } // end CalibrationConsistency check

        }
        /// <summary>
        /// Basic formulate to get the 'slope' of a line, given 2 points
        /// </summary>
        /// <param name="p1">A point on the line</param>
        /// <param name="p2">A second point on the same line</param>
        /// <returns>the slope of the line</returns>
        private double GetSlope(Point p1, Point p2)
        {
            return (p1.Y - p2.Y) / (p1.X - p2.X);
        }
        /// <summary>
        /// Basic formula to get the "Y intercept" or a line
        /// </summary>
        /// <param name="p1">Point on the line</param>
        /// <param name="slope">The slope of the line</param>
        /// <returns>The location of the Y axis where this line crosses</returns>
        private double GetYIntercept(Point p1, double slope)
        {
            return p1.Y - (slope * p1.X);
        }

        #endregion

        /// <summary>
        /// Takes in an image frame, analyzes each pixel, and builds our calibration grid from it.        
        /// </summary>
        /// <param name="imageFrame"></param>
         public void ProcessImageFrameForCalibration(DepthImageFrame imageFrame)
        {
            bool IsCalibrated = true;

            if (calibrationGrid == null)
            {
                calibrationGrid = new CalibrationPixel[imageFrame.PixelDataLength];
                CalibrationImage = new byte[imageFrame.PixelDataLength * 4];
                DepthData = new short[imageFrame.PixelDataLength];
            }

            imageFrame.CopyPixelDataTo(DepthData);
            // For each pixel in the depth image, get the depth and store it as a calibration pixel
            // Note: Do we need to do an x/y here!?
            Parallel.For(0,
                imageFrame.Height, yIndex =>
                {
                    int heightOffset = yIndex * imageFrame.Width;

                    for (int x = 0;
                        x < imageFrame.Width; x++)
                    {
                        int index = (x + heightOffset) * 4;

                        int calibrationIndex = x + heightOffset;
                        int distance = DepthData[calibrationIndex] >> DepthImageFrame.PlayerIndexBitmaskWidth;

                        // First case: No data
                        if ((distance <= 0) && calibrationGrid[calibrationIndex] == null)
                        {
                            CalibrationImage[index] = 255; // Stores as blue
                            IsCalibrated = false;
                        }
                        else
                        {
                            if (distance > 0)
                            {
                                // We have valid data! Color this Green, and calculate it in with what we have/had.
                                CalibrationImage[index + 1] = 255;
                                if (calibrationGrid[calibrationIndex] == null)
                                {
                                    calibrationGrid[calibrationIndex] = new CalibrationPixel(distance,depthManagerSettings);
                                    IsCalibrated = false; // new data coming in, can't be calibrated yet!
                                }
                                else
                                {
                                    IsCalibrated = calibrationGrid[calibrationIndex].Add(distance);                                    
                                    CalibrationImage[index] = 0;
                                    CalibrationImage[index + 1] = 0;
                                    CalibrationImage[index + 2] = 255;                                    
                                }
                            }
                        }
                    } // end innerFor
                }); // end Parallel For
            if (IsCalibrated)
            {
                StopCalibration();
                //KSAConfig.SaveCalibration();
            }
        } // end ProcessImageFrame
    }
}
